/*
  VEXREDUS PARACHUTE CONTROL for ATtiny 45
 */

// Pin assignments
//#define ALIVE_PIN 1 // FIXME  *************** CHANGE BEFORE UPLOADING TO TINY!! ***************
#define ALIVE_PIN 2
#define RELEASE_PIN 4
#define MAGNET_PIN 3

// START CONFIG HERE!!!!!!!!!!!!!!!!
//#define DEBUGGING 1 // FIXME *************** TURN OFF BEFORE UPLOADING TO TINY!! ***************
#define DEBUGGING 0
#define RELEASE_ON_DEAD_TIME 1500 // not! milliseconds, gap time for which alive signal gets identified as dead in 10ms, i.e. 100 = 1s
#define RELEASE_SIGNAL_TIME_TO_RELEASE 5000 // in milliseconds, time release signal has to be HIGH to release the parachute
// END CONFIG HERE!!!!!!!!!!!!!!!!

boolean sequenzer_updated = false;
long time_last = 0;

int alive_pin_current = LOW;
int alive_pin_last = LOW;
int alive_counter = 0;

boolean release_pin_high = false;
long release_signal_start_time = 0;
boolean release_command = false;

// ************************************ SETUP **************************************
void setup() {
  #if DEBUGGING == 1
  Serial.begin(115200);
  #endif
  
  pinMode(ALIVE_PIN, INPUT);
  pinMode(MAGNET_PIN, OUTPUT); //to magnet and LED
  pinMode(RELEASE_PIN, INPUT); //release commnad

  // Set magnet to high (default: hold parachute)
  digitalWrite(MAGNET_PIN, HIGH);
}

// **************************** CHECKS THE ALIVE SIGNAL ****************************
void check_alive_signal() {
  // Alive checker
  alive_pin_current = digitalRead(ALIVE_PIN);
  
  // Check if alive state has changed
  if(((alive_pin_current == HIGH) && (alive_pin_last == LOW)) || ((alive_pin_current == LOW) && (alive_pin_last == HIGH))) {
    // Reset alive counter
    alive_counter = 0;
    
    // Save last state of pin
    alive_pin_last = alive_pin_current;
  }
  // No alive change --> increment alive counter
  else {
    // Make sure alive_counter maximum is RELEASE_ON_DEAD_TIME + 1
    if(alive_counter <= RELEASE_ON_DEAD_TIME) {
      alive_counter++;
    }
    else {
      alive_counter = RELEASE_ON_DEAD_TIME + 1;
    }
  }
  
  #if DEBUGGING == 1
  Serial.print("Alive counter = ");
  Serial.print(alive_counter);
  Serial.print(", alive_pin_current = ");
  if(alive_pin_current == HIGH) Serial.println("HIGH");
  else Serial.println("LOW");
  #endif
}

// **************************** CHECKS THE RELEASE SIGNAL ****************************
void check_release_signal() {
 // Check if release command has been sent from APM
 if(digitalRead(RELEASE_PIN) == HIGH) {
   // Only set the release time once
   if(release_pin_high == false) {
     //Serial.println("******************************* RELEASE PIN IS HIGH ****************************");
     release_signal_start_time = millis();
     release_pin_high = true;
   }
   
   // If release has been HIGH for the set time -> START RELEASING
   if((millis() - release_signal_start_time) > RELEASE_SIGNAL_TIME_TO_RELEASE) {
     release_command = true;
   }
 }
 // No release from APM
 else { // => digitalRead(RELEASE_PIN) == LOW
   // Reset the release
   release_command = false;
   release_pin_high = false;
   //digitalWrite(MAGNET_PIN, HIGH);
 }
 

}

// *********************** CHECK IF PARACHUTE IS TO BE RELEASED ***********************
void check_release_parachute() {
  // Check if its time to release parachute    
  if((alive_counter > RELEASE_ON_DEAD_TIME) || (release_command == true)) {
    // RELEASE THE PARACHUTE
    digitalWrite(MAGNET_PIN, LOW);
    
    #if DEBUGGING == 1
    Serial.print("DEBUG: Releasing! Due to: ");
    if(alive_counter > RELEASE_ON_DEAD_TIME) Serial.print("alive counter | ");
    else Serial.print("release command. | ");
    #endif
    
  }
  else {
    digitalWrite(MAGNET_PIN, HIGH);
  }
}

// ******************************* RELEASE RESET COUNTER *******************************
/*void release_reset_counter() {
  // Increment reset timer
  reset_time_counter++;
  
  // Check if its time to reset the release
  if(reset_time_counter > reset_time_after_release) {
    release_counter = 0;
    release_loop_counter = 0;
    reset_time_counter = 0;
    released = false;
  }
}*/

// ************************************* MAIN LOOP **************************************
void loop() {
  // Sequencer (execute every 10ms)
  if((millis()-time_last) > 10) {
    time_last = millis();
    
    // Check if release command has been sent from APM
    check_release_signal();
    
    // Check the alive signal
    check_alive_signal();
    
    // Check if the parachute should be released
    check_release_parachute();
    
    // If parachute is releasing => execute reset timer
    /*if(released == true) {
      release_reset_counter();
    }*/
    
  }
}
